#ifndef __ROBOT_ARM_H
#define __ROBOT_ARM_H

#include <math.h>
#include <stdio.h>
#include "step_motor.h"
#include "key.h"

#define ROBOT_ARM_INIT_SPEED 20
#define ROBOT_ARM_INIT_AXLE_1 380  // 默认380
#define ROBOT_ARM_INIT_AXLE_2 -132
#define ROBOT_ARM_INIT_AXLE_3 298
#define ROBOT_ARM_BASE_GEAR_RATIO 4.5f
#define ROBOT_ARM_LARM_GEAR_RATIO -6.0f
#define ROBOT_ARM_RARM_GEAR_RATIO 6.0f
#define ROBOT_BIAS_ADJUST 1
#define ROBOT_BIAS 3

#define ROBOT_ARM_DELAY(time) rt_thread_delay(time)

#define AUTOMATIC_RESET 1 

typedef struct {
	float arm_length_1;
	float arm_length_2;
	float arm_length_3;
	float axle_angle[3];
} RobotAngle_Argument_t;

typedef struct{
	float x;
	float y;
	float z;
	uint16_t time;
} RobotArmMove_t;

typedef struct{
	uint16_t num;
	uint16_t pwm;
	uint16_t tim;
} RobotArmClamp_t;

typedef struct{
	float x;
	float y;
	float z;
	uint16_t rtim;
	uint16_t num;
	uint16_t pwm;
	uint16_t ctim;
} RobotArmRun_t;

typedef enum {
	Not_Error,
	A0_Error,
	A1_Error,
	A2_Error
}RobotAngle_Error_t;

extern RobotAngle_Argument_t RobotAngle_Argument[2];

float robotArm_cos(float angle);
float robotArm_sin(float angle);
float robotArm_square(float val);
float robotArm_acos(float radian);
float robotArm_atan(float radian);
void robotArm_init(uint8_t index);
void robotArm_control(uint8_t index,uint16_t speed);
void robotArm_getAngle(uint8_t index,float * angle0,float * angle1,float * angle2);
int robotArm_forwardKinematics(float* x, float* y, float* z, RobotAngle_Argument_t robot_argument);
int robotArm_kinematics(float x, float y, float z, RobotAngle_Argument_t *robot_argument);
void robotArm_control_time(uint8_t index,uint16_t time);
int robotArm_kinematics_move(uint8_t index,float x, float y, float z,uint16_t time);
int robotArm_dataDismantle(uint8_t * pData,RobotArmMove_t * robotArm_move,RobotArmClamp_t * robotArmClamp);
int robotArm_floatEquals(float a, float b, float epsilon);
int robotArm_kinematicsControl(uint8_t index,RobotArmMove_t robotArmMove,RobotArmClamp_t robotArmClamp);
int8_t robotArm_getRunOK(uint8_t index,float bias);
int robotArm_kinematicsCMDControl(uint8_t * cmd);
int8_t robotArm_actionGroup(uint8_t index,RobotArmRun_t * robotArmRun,uint16_t num);
int robotArm_kinematicsRun(uint8_t index,RobotArmRun_t robotArmRun);
//RobotArmRun_t robotArm_assign(float x,float y,float z,uint16_t rtim,uint16_t num,uint16_t pwm,uint16_t ctim);
RobotArmRun_t robotArm_assign(float x,float y,float z,uint16_t rtim,uint16_t num);
int robotArm_coordDismantle(uint8_t *pData,float * x,float * y,float * type);
float robotArm_PID(float error);
void robotArm_AngleGo(float angle,float length,RobotArmRun_t * robotArmRun);

#endif
